#include "prj_config.h"
#include "MPU6050.h"

Adafruit_MPU6050 mpu;
String rec_data;

bool MPU6050_init()
{
    pinMode(14, OUTPUT);
    digitalWrite(14, LOW);
    delay(50);
    Serial.println("Adafruit MPU6050 test!");
    Wire.begin(21, 22, 400000);
    if (!mpu.begin())
    {
        Serial.println("Failed to find MPU6050 chip");
        while (1)
        {
            delay(10);
            if (Serial.available())
            {
                delay(10);
                rec_data = Serial.readString();
                Serial.println(rec_data);
                if (rec_data.indexOf("reboot") != -1)
                    esp_restart();
            }
        }
    }
    Serial.println("MPU6050 Found!");
    mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
    mpu.setGyroRange(MPU6050_RANGE_500_DEG);
    mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
    // setupt motion detection
    // mpu.setHighPassFilter(MPU6050_HIGHPASS_0_63_HZ);
    // mpu.setMotionDetectionThreshold(1);
    // mpu.setMotionDetectionDuration(20);
    // mpu.setInterruptPinLatch(true); // Keep it latched.  Will turn off when reinitialized.
    // mpu.setInterruptPinPolarity(true);
    // mpu.setMotionInterrupt(true);
    delay(10);
    return true;
}

MPU_DATA_t MPU6050_get_data()
{
    MPU_DATA_t data;
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    data.ax = a.acceleration.x;
    data.ay = a.acceleration.y;
    data.az = a.acceleration.z;
    data.gx = g.gyro.x;
    data.gy = g.gyro.y;
    data.gz = g.gyro.z;
    data.temperature = temp.temperature;
    return data;
}

void MPU6050_print_data(MPU_DATA_t temp)
{
    Serial.print("AccelX:");
    Serial.print(temp.ax);
    Serial.print(",");
    Serial.print("AccelY:");
    Serial.print(temp.ay);
    Serial.print(",");
    Serial.print("AccelZ:");
    Serial.print(temp.az);
    Serial.print(", ");
    Serial.print("GyroX:");
    Serial.print(temp.gx);
    Serial.print(",");
    Serial.print("GyroY:");
    Serial.print(temp.gy);
    Serial.print(",");
    Serial.print("GyroZ:");
    Serial.print(temp.gz);
    Serial.println("");
    Serial.print("TEMP:");
    Serial.print(temp.temperature);
    Serial.println("");
}